Class FeatureManager

Class Documentation

class FeatureManager

This class stores and calculates observed landmarks into map points. Triangulate feature points and get initial pose estimation after feature tracking FeatureTracker::trackImage().

Public Functions

explicit FeatureManager(Matrix3d _Rs[])
void setRic(Matrix3d _ric[])

Update transform between camera and IMU/INS.

Parameters

_ric – Input extrinsic calibration matrix.

void clearState()

Clear all the map point memory when resetting.

int getFeatureCount()

Get valid map point number.

Note

Valid means used_num >= 4.

Returns

Map point feature size.

bool addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)

Add the feature output from trackImage() to feature manager.

Parameters

  • frame_count – The index of frame inside sliding window in slam estimator.

  • image – Not the real “image” but the std::map containing all the feature properties and camera ID as index.

  • td – Time interval between current and previous message time stamp.

Returns

True if current frame satisfies the keyframe criteria.

vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r)

Get the vector of correspondences of frame pairs.

Parameters

  • frame_count_l – Frame index 1.

  • frame_count_r – Frame index 2.

Returns

Correspondence of points based on input frame indices.

void setDepth(const VectorXd &x)

Set the updated estimated depth into map point database.

Parameters

x – The estimated inverse depth after optimization.

void removeFailures()

Erase the invalid map points (like estimated depth < 0)

void clearDepth()

Clear all the records of map point estimated depth with -1.

VectorXd getDepthVector()

Acquire vector of depth for each valid map point (used_num >= 4).

Returns

Vector of inverse estimated depth.

void triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])

Triangulates and calculates the estimated depth for each map point inside feature.

Parameters

  • frameCnt – The index of frame inside sliding window in slam estimator.

  • Ps – Vector of estimated translation vector from all sliding window key frames to world coordinate.

  • Rs – Vector of estimated rotation matrices from all sliding window key frames to world coordinate.

  • tic – Extrinsic translation vector from camera to IMU/INS body frame.

  • ric – Extrinsic rotation matrix from camera to IMU/INS body frame.

void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
Parameters

  • Pose0 – Pose of a point at frame 1, from camera plane to world coordinate.

  • Pose1 – Pose of a point at frame 2, from camera plane to world coordinate.

  • point0 – 2D point coordinate of a point at frame 1, in camera plane.

  • point1 – 2D point coordinate of a point at frame 2, in camera plane.

  • point_3d – [out] Generated 3D point of this point in world coordinate.

void initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])

Refine the vector of estimated Rs, Ps using EPnP.

Parameters

  • frameCnt – The index of frame inside sliding window in slam estimator.

  • Ps – Vector of estimated translation vector from all sliding window key frames to world coordinate.

  • Rs – Vector of estimated rotation matrices from all sliding window key frames to world coordinate.

  • tic – Extrinsic translation vector from camera to IMU/INS body frame.

  • ric – Extrinsic rotation matrix from camera to IMU/INS body frame.

bool solvePoseByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, vector<cv::Point2f> &pts2D, vector<cv::Point3f> &pts3D)

Refine pose estimation using cv::SolvePnP.

Parameters

  • R_initial – Initial guess of rotation matrix of a frame.

  • P_initial – Initial guess of translation vector of a frame.

  • pts2D – Vector of 2D point coordinates in local camera coordinate of a frame.

  • pts3D – Corresponding 3D point coordinates in world coordinate.

Returns

True if solve PnP is successful.

void removeBackShiftDepth(const Eigen::Matrix3d &marg_R, const Eigen::Vector3d marg_P, const Eigen::Matrix3d new_R, const Eigen::Vector3d new_P)

Remove oldest observation of a map point from feature and update initial estimated depth of each point.

Note

If a map point observation is empty, erase it and update the estimated depth using optimized pose of starting frame.

Parameters

  • marg_R – The old initial rotation matrix in a sliding window waiting to be marginalized out.

  • marg_P – The old initial translation vector in a sliding window waiting to be marginalized out.

  • new_R – The new initial rotation matrix updated.

  • new_P – The new initial translation vector updated.

void removeBack()

Remove oldest observation of a map point from feature.

Note

If the map point observation is empty, erase it directly.

void removeFront(int frame_count)

Remove the frontest observation of a map point from feature.

Parameters

frame_count – The index of current frame.

void removeOutlier(set<int> &outlierIndex)

Erase outlier map point from feature.

Parameters

outlierIndex –

Public Members

list<FeaturePerId> feature

Database of all the map points with feature properties. List of FeaturePerId.

int last_track_num

Number of map points that have been observed before.

double last_average_parallax
int new_feature_num

Number of new observed map points.

int long_track_num

Number of map points that have been observed for more than /c times.